#include <ros/ros.h>
#include <ros/time.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>
#include <nav_msgs/Odometry.h>
#include <boost/asio.hpp>
#include "tf2_ros/transform_broadcaster.h"
#include <geometry_msgs/Quaternion.h>
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2/LinearMath/Quaternion.h"
#include <array>
#include <thread>

using namespace std;
using namespace boost::asio;

bool ttt;

std_msgs::String msg;
std::stringstream ss;

class MyNode {

public:
    std::string Node;
    MyNode() 
      : Node("node"), 
        sp(iosev, "/dev/ttyUSB1"), 
        
        odom_pose_covariance_({
        {1e-9, 0, 0, 0, 0, 0, 
        0, 1e-3,1e-9, 0, 0, 0, 
        0, 0, 1e6, 0, 0, 0,
        0, 0, 0, 1e6, 0, 0, 
        0, 0, 0, 0, 1e6, 0, 
        0, 0, 0, 0, 0, 1e-9}}),
        
        odom_twist_covariance({
        {1e-9, 0, 0, 0, 0, 0, 
        0, 1e-3,1e-9, 0, 0, 0, 
        0, 0, 1e6, 0, 0, 0, 
        0, 0, 0, 1e6, 0, 0, 
        0, 0, 0, 0, 1e6, 0, 
        0, 0, 0, 0, 0, 1e-9}})
    
    {
        serialInit();
        
        // Create Odometry publisher
        odom_publisher = nh_.advertise<nav_msgs::Odometry>("odom", 1);
        
        // Create TransformBroadcaster
        tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>();
        
        //自己添加的代码
        subscription2_ = nh_.subscribe("/cmd_vel", 20, &MyNode::cmdVelCallback, this);
        
    }
    
    void controlLoop();

private:
    boost::asio::io_service iosev;
    boost::asio::serial_port sp;    
    ros::NodeHandle nh_; 
    ros::Timer timer;
    ros::Publisher odom_publisher;
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;    

    const unsigned char header[2]  = {0x55, 0xaa};
    const unsigned char ender[2]   = {0x0d, 0x0a};

    union sendData {
        short d;
        unsigned char data[2];
    } leftVelSet, rightVelSet;

    union receiveData {
        short d;
        unsigned char data[2];
    } leftVelNow, rightVelNow, angleNow;

    unsigned char getCrc8(unsigned char *ptr, unsigned short len) {
        unsigned char crc;
        unsigned char i;
        crc = 0;
        while (len--) {
            crc ^= *ptr++;
            for (i = 0; i < 8; i++) {
                if (crc & 0x01)
                    crc = (crc >> 1) ^ 0x8C;
                else
                    crc >>= 1;
            }
        }
        return crc;
    }

    void serialInit() {
        sp.set_option(serial_port::baud_rate(115200));
        sp.set_option(serial_port::flow_control(serial_port::flow_control::none));
        sp.set_option(serial_port::parity(serial_port::parity::none));
        sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
        sp.set_option(serial_port::character_size(8));    
    }

    void writeSpeed(double Left_v, double Right_v,unsigned char ctrlFlag) {
        unsigned char buf[13] = {0};//
        int i, length = 0;
    
        leftVelSet.d  = Left_v;//mm/s
        rightVelSet.d = Right_v;
    

        for(i = 0; i < 2; i++)
            buf[i] = header[i];             //buf[0]  buf[1]
        
    
        length = 5;
        buf[2] = length;                    //buf[2]
        for(i = 0; i < 2; i++) {
            buf[i + 3] = leftVelSet.data[i];  //buf[3] buf[4]
            buf[i + 5] = rightVelSet.data[i]; //buf[5] buf[6]
        }

        buf[3 + length - 1] = ctrlFlag;       //buf[7]
    

        buf[3 + length] = getCrc8(buf, 3 + length);//buf[8]
        buf[3 + length + 1] = ender[0];     //buf[9]
        buf[3 + length + 2] = ender[1];     //buf[10]
    

        boost::asio::write(sp, boost::asio::buffer(buf));
    }

    bool readSpeed(double &Left_v,double &Right_v,double &Angle,unsigned char &ctrlFlag) {
        unsigned char i, length = 0;
        unsigned char checkSum;
        unsigned char buf[1024]={0};
        boost::system::error_code err; 

        try {
            boost::asio::streambuf response;
            boost::asio::read_until(sp, response, "\r\n",err);  
            copy(istream_iterator<unsigned char>(istream(&response)>>std::noskipws), istream_iterator<unsigned char>(), buf); 
        } catch(boost::system::system_error &err) {
            ROS_INFO("read_until error");
            return false;
        } 
    

        if (buf[0]!= header[0] || buf[1] != header[1]) {
            ROS_INFO("Received message header error!");
            return false;
        }

        length = buf[2];                                
    

        checkSum = getCrc8(buf, 3 + length);             
        if (checkSum != buf[3 + length]) {                
            ROS_INFO("Received data check sum error!");
            return false;
        }    
    

        for(i = 0; i < 2; i++) {
            leftVelNow.data[i]  = buf[i + 3]; //buf[3] buf[4]
            rightVelNow.data[i] = buf[i + 5]; //buf[5] buf[6]
            angleNow.data[i]    = buf[i + 7]; //buf[7] buf[8]
        }
    

        ctrlFlag = buf[9];
        
        Left_v  =leftVelNow.d;
        Right_v =rightVelNow.d;
        Angle   =angleNow.d;
     
        return true;
    }

    boost::array<double, 36> odom_pose_covariance_;
    boost::array<double, 36> odom_twist_covariance;
    
    //自己添加
    ros::Subscriber subscription2_;
    
        //订阅速度节点的代码
    void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg) {
       double linear_vel_x = (msg->linear.x) *100;
       ROS_INFO("msg->linear.x:%.2f",msg->linear.x);
       double angular_vel_th = (msg->angular.z)*2;
       double right_speed = 0.0;
       double left_speed = 0.0;
   
	    right_speed = (linear_vel_x + angular_vel_th*10.0);
	    left_speed = (linear_vel_x - angular_vel_th*10.0);
   
       writeSpeed(left_speed,right_speed,0x01);
        
       ROS_INFO("daohang:right_speed=%.2f,left_speed=%.2f",right_speed/100, left_speed/100);
    }
};

void MyNode::controlLoop() {
    static double position_x = 0.0;
    static double position_y = 0.0;
    static double position_x_last = 0.0;
    static double position_y_last = 0.0;
    static double center_d = 0.0;
    static double delta_th = 0.0;
    static double duration = 0.0;
    
    double left_speed_now = 0.0;
    double right_speed_now = 0.0;
    double angle = 0.0;
    double angle_last = 0.0;
    unsigned char testRece4 = 0x00;
    double linear = 0.0;   
    static ros::Time time_last = ros::Time::now();
    
    ros::Time time_now = ros::Time::now();

    readSpeed(left_speed_now, right_speed_now, angle, testRece4);
    static int aaa = 0;
    aaa++;
    if(aaa>=20)//一秒钟显示一遍下位机数据
    {
      aaa=0;
      ROS_INFO("left_speed_now=%.2f,right_speed_now =%.2f,angle = %.2f", left_speed_now,right_speed_now,angle);
    }
    
    double delta_th_rad = angle * (M_PI / 180.0);    //角度转弧度
    duration = (time_now - time_last).toSec();   //时间间隔：s
    center_d =  (left_speed_now*duration + right_speed_now*duration) / 200;    //中心点位移
    position_x = position_x_last + center_d *cos(delta_th_rad - angle_last) ;    //位置
    position_y = position_y_last + center_d *sin(delta_th_rad - angle_last) ;
    linear = (left_speed_now + right_speed_now) / 200;    //线速度
    delta_th = (delta_th_rad - angle_last)*duration;    //角速度
    
    //保存当前值作为下一次的值
    angle_last = delta_th_rad;
    position_y_last = position_y;
    position_x_last = position_x;

    //下面填充里程计数据并发布
    tf2::Quaternion odom_quat_tf2;
    odom_quat_tf2.setRPY(0, 0, delta_th_rad);
    
    nav_msgs::Odometry msg;
    msg.header.stamp = ros::Time::now();
    msg.header.frame_id = "odom";

    msg.pose.pose.position.x = position_x;
    msg.pose.pose.position.y = position_y;
    msg.pose.pose.position.z = 0.0;

    msg.pose.pose.orientation = tf2::toMsg(odom_quat_tf2);
    
    for (int i = 0; i < 36; ++i) {
        msg.pose.covariance[i] = odom_pose_covariance_[i];
    }

    msg.child_frame_id = "base_footprint";

    msg.twist.twist.linear.x = linear;
    msg.twist.twist.linear.y = 0.0;
    msg.twist.twist.angular.z = delta_th;
    
    for (int i = 0; i < 36; ++i) {
        msg.twist.covariance[i] = odom_twist_covariance[i];
    }
    
    odom_publisher.publish(msg);

    geometry_msgs::TransformStamped odom_trans;
    /*geometry_msgs::msg::TransformStamped odom_trans;*/
    odom_trans.header.stamp = ros::Time::now();
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_footprint";

    odom_trans.transform.translation.x = position_x;
    odom_trans.transform.translation.y = position_y;
    odom_trans.transform.translation.z = 0.0;
    
    odom_trans.transform.rotation = tf2::toMsg(odom_quat_tf2);
    tf_broadcaster->sendTransform(odom_trans);
    
    time_last = time_now;

}

int main(int argc, char **argv) {
    ros::init(argc, argv, "Mynode");
    ros::NodeHandle nh;
    /*rclcpp::init(argc, argv);*/

    auto node = std::make_shared<MyNode>();
    
    std::thread control_thread([&]() {
        ros::Rate loop_rate(20);
        /*rclcpp::Rate loop_rate(20);*/
        while (ros::ok()) {
        /*while (rclcpp::ok()) {*/
            node->controlLoop();
            loop_rate.sleep();
        }
    });
    
    ros::spin();
    /*rclcpp::spin(node);*/

    ros::shutdown();
    /*rclcpp::shutdown();*/

    return 0;
}
